/*
 * JRoverSensors.h
 *
 *  Created on: Nov 22, 2012
 *      Author: bouchier
 */
/*! \file JRoverSensors.h
 * \brief Header file declaring JRoverSensors class
 */
#ifndef JROVERSENSORS_H_
#define JROVERSENSORS_H_

/*! \class JRoverSensors
 * \brief This class provides methods to run JRoverSensors and publish their measurements locally and to ROS
 *
 * First, call initSensors() to initialize the sensor subsystem and state machine. Next, call pollSensors()
 * every 50ms before using the sensor readings, to get updated readings into the class and to start any
 * new ultrasonic range measurement and to make the latest data available in the ROS pubilication array
 */
class JRoverSensors {
public:
	JRoverSensors();
	virtual ~JRoverSensors();
	void initSensors();
	int getRcSpeed();
	int getRcDir();
	int getRcEstop();
	float getBatteryVoltage();
	void pollSensors();
	int getLDistance();
	int getCDistance();
	int getRDistance();

/*! \var robotTelemetry
 * robotTelemetry is an array of floats containing the following data. It is published on the ROS topic
 * robotTelemetry by the ROSIF class.
 * - 0: rosLinear. The linear velocity last requested by ROS
 * - 1: rosAngular. The angular velocity last requested by ROS
 * - 2: rcLinear. The linear velocity last requested by R/C
 * - 3: rcAngular. The angular velocity last requested by R/C
 * - 4: estop. The last value of the R/C estop channel
 * - 5: X. Current X position
 * - 6: Y. Current Y position
 * - 7: theta: Current angle relative to start
 * - 8: Battery voltage of VEXPro battery in volts
 * - 9: Left ultrasonic range measurement in meters
 * - 10: Center ultrasonic range measurement in meters
 * - 11: Right ultrasonic range measurement in meters
 */
	float robotTelemetry[20];
	int getRobotTelemetrySize() {return 11;}


private:
	void updateOdometers();

	struct RCChannel *dirRcc;
	struct RCChannel *speedRcc;
	struct RCChannel *estopRcc;
	int currentUsonicRanger;			// the ultrasonic ranger that is currently doing a ranging measurement
	struct usonicRanger *lRanger;		// left ranger
	struct usonicRanger *cRanger;		// center ranger
	struct usonicRanger *rRanger;		// right ranger
	float voltage;						// main battery voltage

	float lmDistance;					// left middle drive encoder distance
	float rmDistance;					// right middle drive encoder distance
	float lastLMDistance;				// last left-middle distance
	float lastRMDistance;				// last right-middle distance
	float X, Y, theta;					// new pose

};

#endif /* JROVERSENSORS_H_ */
